#include "command_handler.h"
#include "mcnamara.h"
#include "uart0.h"
#include "esp_log.h"
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "CMD_HANDLER";

#define MAX_CMD_LEN 32
#define MAX_PARAM_LEN 16

// 命令缓冲区
static char cmd_buffer[MAX_CMD_LEN];
static uint8_t cmd_index = 0;

// 解析命令和参数
static bool parse_command(const char *cmd_str, char *cmd, char *params) {
    char *colon_pos = strchr(cmd_str, ':');
    
    // 如果没有冒号，可能是不带参数的命令
    if (colon_pos == NULL) {
        strcpy(cmd, cmd_str);
        params[0] = '\0';
        return true;
    }
    
    // 复制命令部分
    int cmd_len = colon_pos - cmd_str;
    strncpy(cmd, cmd_str, cmd_len);
    cmd[cmd_len] = '\0';
    
    // 复制参数部分
    strcpy(params, colon_pos + 1);
    
    return true;
}

// 处理移动命令
static void handle_move_command(const char *cmd, const char *params) {
    int speed = 0;
    int deflection = 0;
    int rate = 0;
    char *token = NULL;
    
    // 解析速度参数
    if (params[0] != '\0') 
    {
        speed = atoi(params);
        
        // 确保速度在有效范围内
        if (speed > PWM_MOTOR_MAX_VALUE) speed = PWM_MOTOR_MAX_VALUE;
        if (speed < 0)   speed = 0;
    }
    
    // 处理不同的命令
    if (strcmp(cmd, "FWD") == 0) {
        ESP_LOGI(TAG, "Moving forward with speed %d", speed);
        move_forward(speed);
    } 
    else if (strcmp(cmd, "BWD") == 0) {
        ESP_LOGI(TAG, "Moving backward with speed %d", speed);
        move_backward(speed);
    }
    else if (strcmp(cmd, "LFT") == 0) {
        ESP_LOGI(TAG, "Moving left with speed %d", speed);
        move_left(speed);
    }
    else if (strcmp(cmd, "RGT") == 0) {
        ESP_LOGI(TAG, "Moving right with speed %d", speed);
        move_right(speed);
    }
    else if (strcmp(cmd, "RLF") == 0) {
        ESP_LOGI(TAG, "Rotating left with speed %d", speed);
        rotate_left(speed);
    }
    else if (strcmp(cmd, "RRT") == 0) {
        ESP_LOGI(TAG, "Rotating right with speed %d", speed);
        rotate_right(speed);
    }
    else if (strcmp(cmd, "DLF") == 0) {
        ESP_LOGI(TAG, "Moving diagonal left front with speed %d", speed);
        move_diagonal_left_front(speed);
    }
    else if (strcmp(cmd, "DLB") == 0) {
        ESP_LOGI(TAG, "Moving diagonal left back with speed %d", speed);
        move_diagonal_left_back(speed);
    }
    else if (strcmp(cmd, "DRF") == 0) {
        ESP_LOGI(TAG, "Moving diagonal right front with speed %d", speed);
        move_diagonal_right_front(speed);
    }
    else if (strcmp(cmd, "DRB") == 0) {
        ESP_LOGI(TAG, "Moving diagonal right back with speed %d", speed);
        move_diagonal_right_back(speed);
    }
    else if (strcmp(cmd, "DFT") == 0) {
        // 使用栈上的缓冲区而不是动态分配内存
        char local_params[MAX_PARAM_LEN];
        strncpy(local_params, params, MAX_PARAM_LEN - 1);
        local_params[MAX_PARAM_LEN - 1] = '\0';

        // 解析第一个参数：速度
        token = strtok(local_params, ",");
        if (!token) {
            ESP_LOGW(TAG, "DFT command: Missing speed parameter");
            return;
        }
        speed = atoi(token);
        if (speed < 0) speed = 0;
        if (speed > 100) speed = 100;

        // 解析第二个参数：偏转
        token = strtok(NULL, ",");
        if (!token) {
            ESP_LOGW(TAG, "DFT command: Missing deflection parameter");
            return;
        }
        deflection = atoi(token);

        // 解析第三个参数：比率
        token = strtok(NULL, ",");
        if (!token) {
            ESP_LOGW(TAG, "DFT command: Missing rate parameter");
            return;
        }
        rate = atoi(token);

        ESP_LOGI(TAG, "Drifting with speed %d, deflection %d, rate %d", 
                speed, deflection, rate);
        drifting(speed, deflection, rate);

        // 漂移命令处理完成后短暂让出CPU
        vTaskDelay(1);
    }
    else if (strcmp(cmd, "STP") == 0) {
        ESP_LOGI(TAG, "Stopping all movement");
        move_forward(0); // 速度为0表示停止
    }
    else {
        ESP_LOGW(TAG, "Unknown command: %s", cmd);
    }
}

// 处理完整的命令
static void process_command(const char *cmd_str) {
    char cmd[MAX_CMD_LEN] = {0};
    char params[MAX_PARAM_LEN] = {0};
    
    ESP_LOGI(TAG, "Processing command: %s", cmd_str);
    
    if (parse_command(cmd_str, cmd, params)) {
        handle_move_command(cmd, params);
    } else {
        ESP_LOGW(TAG, "Failed to parse command: %s", cmd_str);
    }
}

// 初始化命令处理模块
void Command_Handler_Init(void) {
    ESP_LOGI(TAG, "Command handler initialized");
    cmd_index = 0;
    memset(cmd_buffer, 0, MAX_CMD_LEN);
}

// 处理接收到的命令
void Command_Handler_Process(void) {
    // 限制每次处理的字符数量，避免长时间占用CPU
    const int MAX_CHARS_PER_CALL = 64;
    int chars_processed = 0;
    
    while (Uart0_Available() > 0 && chars_processed < MAX_CHARS_PER_CALL) {
        uint8_t c = Uart0_Read();
        chars_processed++;
        
        // 如果收到换行符，处理命令
        if (c == '\n' || c == '\r') {
            if (cmd_index > 0) {
                cmd_buffer[cmd_index] = '\0';
                process_command(cmd_buffer);
                cmd_index = 0;
                
                // 处理完一个命令后短暂让出CPU
                vTaskDelay(1);
            }
        }
        // 否则，将字符添加到缓冲区
        else if (cmd_index < MAX_CMD_LEN - 1) {
            cmd_buffer[cmd_index++] = c;
        }
        // 缓冲区溢出，重置
        else {
            cmd_index = 0;
            ESP_LOGW(TAG, "Command buffer overflow");
        }
        
        // 每处理16个字符短暂让出CPU
        if (chars_processed % 16 == 0) {
            vTaskDelay(1);
        }
    }
}